#ifndef SCAN_GROUND_FILTER_HPP_
#define SCAN_GROUND_FILTER_HPP_

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <cmath>
#include <algorithm>
#include <limits>
#include <vector>

namespace autoware_ground_segmentation
{

// 点标签枚举
enum class PointLabel : uint16_t {
  INIT = 0,
  GROUND,
  NON_GROUND,
  POINT_FOLLOW,
  UNKNOWN,
  VIRTUAL_GROUND,
  OUT_OF_RANGE
};

// 点数据结构
struct PointData
{
  float radius;  // XY平面上的圆柱坐标半径
  PointLabel point_state{PointLabel::INIT};
  uint16_t grid_id;   // 垂直网格ID
  size_t data_index;  // 源点云中此点数据的索引
};

using PointCloudVector = std::vector<PointData>;

class ScanGroundFilter
{
public:
  ScanGroundFilter(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
  ~ScanGroundFilter() = default;

private:
  // 回调函数
  void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
  
  // 核心处理函数
  void convertPointcloud(const sensor_msgs::PointCloud2::ConstPtr& in_cloud,
                        std::vector<PointCloudVector>& out_radial_ordered_points) const;
  void classifyPointCloud(const sensor_msgs::PointCloud2::ConstPtr& input_msg,
                         const std::vector<PointCloudVector>& radial_ordered_points,
                         pcl::PointIndices& no_ground_indices);
  void extractObjectPoints(const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
                          const pcl::PointIndices& in_indices,
                          sensor_msgs::PointCloud2& out_object_cloud) const;
  
  // 辅助函数
  void calcVirtualGroundOrigin(pcl::PointXYZ& point) const;
  
  // ROS相关
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  tf::TransformListener tf_listener_;
  tf2_ros::Buffer tf2_buffer_;
  tf2_ros::TransformListener tf2_listener_;
  
  // 订阅和发布
  ros::Subscriber pointcloud_sub_;
  ros::Publisher ground_cloud_pub_;
  ros::Publisher no_ground_cloud_pub_;
  
  // 参数
  bool elevation_grid_mode_;
  float non_ground_height_threshold_;
  float low_priority_region_x_;
  float center_pcl_shift_;
  
  float radial_divider_angle_rad_;
  size_t radial_dividers_num_;
  float global_slope_max_angle_rad_;
  float local_slope_max_angle_rad_;
  float global_slope_max_ratio_;
  float local_slope_max_ratio_;
  float split_points_distance_tolerance_;
  
  bool use_virtual_ground_point_;
  float split_height_distance_;
  
  bool use_recheck_ground_cluster_;
  float recheck_start_distance_;
  bool use_lowest_point_;
  float detection_range_z_max_;
  
  float grid_size_m_;
  float grid_mode_switch_radius_;
  uint16_t gnd_grid_buffer_size_;
  float virtual_lidar_z_;
  
  // PCL过滤器
  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
};

} // namespace autoware_ground_segmentation

#endif // SCAN_GROUND_FILTER_HPP_ 